/*=========================================================================
UberSim Source Code Release
-------------------------------------------------------------------------
Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
                   James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
                   Erick Tryzelaar {erickt}@andrew.cmu.edu
School of Computer Science, Carnegie Mellon University
-------------------------------------------------------------------------
This software is distributed under the GNU General Public License,
version 2.  If you do not have a copy of this licence, visit
www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
in the hope that it will be useful, but WITHOUT ANY WARRANTY,
including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-------------------------------------------------------------------------*/

#include "mmgr.h"

#include "MotionController.h"
#include "Spatial.h"
#include "Node.h"

/*******************************/

MotionController::~MotionController ()
{
	model = 0;
}

/*******************************/

MotionController::MotionController ()
{
	isEnabled = true;
}

/*******************************/

Spatial* MotionController::getModel ()
{
	return model;
}

/*******************************/

Vector3& MotionController::getLocalPosition ()
{
	return model->getLocalPosition ();
}

/*******************************/

Matrix33& MotionController::getLocalRotation ()
{
	return model->getLocalRotation ();
}

/*******************************/

Vector3& MotionController::getLocalVelocity ()
{
	return localVelocity;
}

/*******************************/

Vector3& MotionController::getLocalAngularVelocity ()
{
	return localAngularVelocity;
}

/*******************************/

Vector3& MotionController::getLocalAcceleration ()
{
	return localAcceleration;
}

/*******************************/

Vector3& MotionController::getLocalAngularAcceleration ()
{
	return localAngularAcceleration;
}

/*******************************/

Vector3& MotionController::getWorldPosition ()
{
	return model->getWorldPosition ();
}

/*******************************/

Matrix33& MotionController::getWorldRotation ()
{
	return model->getWorldRotation ();
}

/*******************************/

Vector3& MotionController::getWorldVelocity ()
{
	return worldVelocity;
}

/*******************************/

Vector3& MotionController::getWorldAngularVelocity ()
{
	return worldAngularVelocity;
}

/*******************************/

Vector3& MotionController::getWorldAcceleration ()
{
	return worldAcceleration;
}

/*******************************/

Vector3& MotionController::getWorldAngularAcceleration ()
{
	return worldAngularAcceleration;
}

/*******************************/

void MotionController::setModel (Spatial* model)
{
	if (this->model)
	{
		model->setMotionController (0);
	}
	
	this->model = model;

	if (model)
	{
		model->setMotionController (this);
	}
}

/*******************************/

void MotionController::setLocalPosition (const Vector3& localPosition)
{
	model->setLocalPosition (localPosition);
}

/*******************************/

void MotionController::setLocalRotation (const Matrix33& localRotation)
{
	model->setLocalRotation (localRotation);
}

/*******************************/

void MotionController::setLocalVelocity (const Vector3& localVelocity)
{
	this->localVelocity = localVelocity;
}

/*******************************/

void MotionController::setLocalAngularVelocity (const Vector3& localAngularVelocity)
{
	this->localAngularVelocity = localAngularVelocity;
}

/*******************************/

void MotionController::setLocalAcceleration (const Vector3& localAcceleration)
{
	this->localAcceleration = localAcceleration;
}

/*******************************/

void MotionController::setLocalAngularAcceleration (const Vector3& localAngularAcceleration)
{
	this->localAngularAcceleration = localAngularAcceleration;
}

/*******************************/

void MotionController::setWorldPosition (const Vector3& worldPosition)
{
	model->setWorldPosition (worldPosition);
}

/*******************************/

void MotionController::setWorldRotation (const Matrix33& worldRotation)
{
	model->setWorldRotation (worldRotation);
}

/*******************************/

void MotionController::setWorldVelocity (const Vector3& worldVelocity)
{
	this->worldVelocity = worldVelocity;
}

/*******************************/

void MotionController::setWorldAngularVelocity (const Vector3& worldAngularVelocity)
{
	this->worldAngularVelocity = worldAngularVelocity;
}

/*******************************/

void MotionController::setWorldAcceleration (const Vector3& worldAcceleration)
{
	this->worldAcceleration = worldAcceleration;
}

/*******************************/

void MotionController::setWorldAngularAcceleration (const Vector3& worldAngularAcceleration)
{
	this->worldAngularAcceleration = worldAngularAcceleration;
}

/*******************************/

void MotionController::update (Real deltaTime)
{
	Spatial* parent = model->getParent ();

	if (parent)
	{
		MotionController* motionController = parent->getMotionController ();

		if (motionController)
		{
			worldAcceleration = motionController->getWorldAcceleration () + localAngularAcceleration;

			if (localAngularAcceleration != Vector3::ZERO)
			{
				worldAcceleration = 
					motionController->getWorldAcceleration () + 
					worldAngularAcceleration.cross         (motionController->getLocalPosition ()) +
					worldAngularVelocity.cross             (worldAngularVelocity.cross (motionController->getLocalPosition ())) +
					2.0 * worldAngularVelocity.cross       (localVelocity) +
					localAcceleration;
			}
		}

	}
}

/*******************************/
